#include "Node.h"
#include "SDLittleDog.h"
#include "SDSlowWalk.h"
#include "SDVectorOp.h"
#include <iostream>
#include <ctime>
#include <stdlib.h>
#include <cstdlib>
#include <math.h>
#include <vector>
#include <fstream>
#include <stdio.h>
#include <utility>
#include <stack>
#include <queue>
#include <set>

using namespace std;

//This method prints a node to the terminal - generally used for debugging
void PrintFootPositions(Node* node){
     //printf("\nf: %f\n", node->f);
     bduVec3f* globalFeet = node->foot_pos;
     printf("feet: , (%f, %f, %f) (%f, %f, %f) (%f, %f, %f) (%f, %f, %f)\n", globalFeet[0].n[0],globalFeet[0].n[1],globalFeet[0].n[2],globalFeet[1].n[0], globalFeet[1].n[1],globalFeet[1].n[2],globalFeet[2].n[0],globalFeet[2].n[1],globalFeet[2].n[2],globalFeet[3].n[0],globalFeet[3].n[1],globalFeet[3].n[2]);
}


float* FindNodeCenter(Node* node){
     float* center = new float[2];
     center[0] = 0;
     center[1] = 0;
     for(int i = 0; i < 4; i++){
        center[0] += node->foot_pos[i].n[0];
        center[1] += node->foot_pos[i].n[1];
     }
     center[0] /= 4;
     center[1] /= 4;
     return center;
}

//This method calculates the distance between the indicated feet of the indicated node (dog configuration) in the x-y plain
float FootDist(Node* node, int foot1, int foot2){
     return sqrt(pow(node->foot_pos[foot1].n[0] - node->foot_pos[foot2].n[0],2) + pow(node->foot_pos[foot1].n[1] - node->foot_pos[foot2].n[1],2));
}



